/*==============================================================================
 *
 * Chris Rigg and Sam Strom
 *
 * ECEN 5033
 *
 * Spring 2008
 *
 * Much of the code in capture.c was adapted from original code provided
 * by the following sources:
 *
 * - Video 4 Linux 2 API Example : http://v4l2spec.bytesex.org/ (capture.c)
 *   
 * - libfg source code for Video 4 Linux 1 API : http://antonym.org/libfg
 *   This was mainly used as a reference for our use of SDL
 *
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include <getopt.h>             /* getopt_long() */

#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <SDL/SDL.h>
#include <asm/types.h>            /* for videodev2.h */
#include <linux/videodev2.h>

#include "robot_comm_interface.h" /* for serial interface */
#include "frame_parse.h"          /* for frame processing */
#include "perflib.h"

#define CLEAR(x) memset (&(x), 0, sizeof (x))

typedef enum {
  IO_METHOD_READ,
  IO_METHOD_MMAP,
  IO_METHOD_USERPTR,
} io_method;

struct buffer {
  void * start;
  size_t length;
};

static char *dev_name = NULL;
static io_method io = IO_METHOD_MMAP;
static int fd = -1;
struct buffer *buffers = NULL;
static unsigned int n_buffers = 0;
static int dump_count = 0;
static int cur_frame = 0;
static double DT=0.0, startTOD=0.0, stopTOD=0.0;
static int quit_mainloop = 0;

#define NUM_FRAMES 1000000

static SDL_Surface *screen;
static SDL_Surface *image;

static void init_sdl (void)
{
  unsigned int bmask = 0xff000000;
  unsigned int gmask = 0x00ff0000;
  unsigned int rmask = 0x0000ff00;
  unsigned int amask = 0x000000ff;
  int bpp = 32;

  if ( SDL_Init( SDL_INIT_VIDEO ) < 0 )
  {
    fprintf( stderr, "Failed to initialise SDL: %s\n", SDL_GetError() );
    exit(-1);
  }

  atexit( SDL_Quit );

  // Create a window the size of the display

  SDL_WM_SetCaption( "camview", NULL );

  screen = SDL_SetVideoMode( WIDTH, HEIGHT, bpp, SDL_SWSURFACE );

  if ( screen == NULL )
  {
    fprintf( stderr, "Failed to set video mode: %s\n", SDL_GetError() );
    exit(-1);
  }

  image = SDL_CreateRGBSurfaceFrom( (void*)buffers[0].start,
                                    WIDTH,
                                    HEIGHT,
                                    bpp,
                                    WIDTH * 4,
                                    rmask,
                                    gmask,
                                    bmask,
                                    amask);

  if ( image == NULL )
  {
    fprintf( stderr, "Failed to create RGB surface! %s\n", SDL_GetError());
    exit(-1);
  }

  // Ignore alpha channel, go opaque
  SDL_SetAlpha( image, 0, 0 );
}

static void update_camview( const void *data )
{
  if ( SDL_BlitSurface( image, NULL, screen, NULL ) < 0 )
  {
    fprintf( stderr, "Cannot blit! %s\n", SDL_GetError() );
    return;
  }

  SDL_UpdateRect( screen, 0, 0, 0, 0 );
}

static void
errno_exit (const char *s)
{
  fprintf (stderr,
           "%s error %d, %s\n",
           s,
           errno,
           strerror (errno));

  exit (EXIT_FAILURE);
}

static int
xioctl (int fd, int request, void *arg)
{
  int r;

  do r = ioctl (fd, request, arg);
  while (-1 == r && EINTR == errno);

  return r;
}

static void
process_image (void *p)
{
  int x, y;
  double coverage;
  SDL_Event event;
  static search_mode = 0;

  coverage = find_center(p, 0xf0, 0xff, 0x0, 0xff, 0x0, 0xff, WIDTH, HEIGHT, &x, &y);
  if((x <= ((WIDTH / 2) + CENTER_OFFSET) && x >= ((WIDTH / 2) - CENTER_OFFSET)) &&
     (y <= ((HEIGHT / 2) + CENTER_OFFSET) && y >= ((HEIGHT / 2) - CENTER_OFFSET)) && 
     search_mode)
  {
    rci_stop_search_mode();
    printf("Melo found the ball!!!\n");
    search_mode = 0;
    rci_grab ();
    rci_dunk ();
  }

  add_crosshairs((int *)p, WIDTH, HEIGHT, x, y);
  update_camview(p);

  // Check for SDL event
  while(SDL_PollEvent(&event)) {
     switch(event.type) {
       case SDL_KEYUP:
         if(event.key.keysym.sym == SDLK_d) {
            save_frame(p, WIDTH, HEIGHT, dump_count);
            dump_count++;
         }
         else if(event.key.keysym.sym == SDLK_f) {
            // get the ending time and compute frames/sec
            stopTOD = readTOD();
            DT = elapsedTOD(stopTOD, startTOD);
            printf("Current frames per sec: %3.3f\n", ((double) cur_frame) / DT);
         }
         else if(event.key.keysym.sym == SDLK_r) {
            save_frame_readable (p, WIDTH, HEIGHT, 0);
            save_frame(p, WIDTH, HEIGHT, 0);
            //filter_color((int *)p, WIDTH, HEIGHT, RED);
            filter((char *)p, WIDTH, HEIGHT, 0xf0, 0xff);
            save_frame_readable (p, WIDTH, HEIGHT, 1);
            save_frame(p, WIDTH, HEIGHT, 1);
         }
         else if(event.key.keysym.sym == SDLK_c) {
           printf("Coverage: %.3f - (%d,%d)\n", coverage, x, y);
         }
         else if(event.key.keysym.sym == SDLK_s) {
           search_mode = 1;
           rci_start_search_mode();
         }
         else if(event.key.keysym.sym == SDLK_x) {
           search_mode = 0;
           rci_stop_search_mode();
         }
         else if(event.key.keysym.sym == SDLK_q) {
           printf("Quit the program...\n");
           quit_mainloop = 1;
         }
         else if(event.key.keysym.sym == SDLK_g) {
           rci_grab ();
         }
         else if(event.key.keysym.sym == SDLK_t) {
           rci_dunk ();
         }
         break;
       default:
         break;
     }
  }
}

static int
read_frame (void)
{
  struct v4l2_buffer buf;
  unsigned int i;

  switch (io) {
  case IO_METHOD_READ:
    if (-1 == read (fd, buffers[0].start, buffers[0].length))
    {
      switch (errno) {
      case EAGAIN:
        return 0;

      case EIO:
        /* Could ignore EIO, see spec. */

        /* fall through */

      default:
        errno_exit ("read");
      }
    }

    process_image (buffers[0].start);

    break;

  case IO_METHOD_MMAP:
    CLEAR (buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
      switch (errno) {
      case EAGAIN:
        return 0;

      case EIO:
        /* Could ignore EIO, see spec. */

        /* fall through */

      default:
        errno_exit ("VIDIOC_DQBUF");
      }
    }

    assert (buf.index < n_buffers);

    process_image (buffers[buf.index].start);

    if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
      errno_exit ("VIDIOC_QBUF");

    break;

  case IO_METHOD_USERPTR:
    CLEAR (buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_USERPTR;

    if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
      switch (errno) {
      case EAGAIN:
        return 0;

      case EIO:
        /* Could ignore EIO, see spec. */

        /* fall through */

      default:
        errno_exit ("VIDIOC_DQBUF");
      }
    }

    for (i = 0; i < n_buffers; ++i)
      if (buf.m.userptr == (unsigned long) buffers[i].start
          && buf.length == buffers[i].length)
        break;

    assert (i < n_buffers);

    process_image ((void *) buf.m.userptr);

    if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
      errno_exit ("VIDIOC_QBUF");

    break;
  }

  return 1;
}

static void
mainloop (void)
{
  unsigned int count;

  count = NUM_FRAMES;

  // get the staring time
  startTOD = readTOD();

  while (count-- > 0 && !quit_mainloop) {
    for (;;) {
      fd_set fds;
      struct timeval tv;
      int r;
      
      cur_frame++;

      FD_ZERO (&fds);
      FD_SET (fd, &fds);

      /* Timeout. */
      tv.tv_sec = 2;
      tv.tv_usec = 0;

      r = select (fd + 1, &fds, NULL, NULL, &tv);

      if (-1 == r) {
        if (EINTR == errno)
          continue;

        errno_exit ("select");
      }

      if (0 == r) {
        fprintf (stderr, "select timeout\n");
        exit (EXIT_FAILURE);
      }

      if (read_frame ())
        break;
  
      /* EAGAIN - continue select loop. */
    }
  }

  // get the ending time and compute frames/sec
  stopTOD = readTOD();
  DT = elapsedTOD(stopTOD, startTOD);
  printf("Total frames per sec: %3.3f\n", ((double) cur_frame) / DT);
}

static void
stop_capturing (void)
{
  enum v4l2_buf_type type;

  switch (io) {
  case IO_METHOD_READ:
    /* Nothing to do. */
    break;

  case IO_METHOD_MMAP:
  case IO_METHOD_USERPTR:
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
      errno_exit ("VIDIOC_STREAMOFF");

    break;
  }
}

static void
start_capturing (void)
{
  unsigned int i;
  enum v4l2_buf_type type;

  switch (io) {
  case IO_METHOD_READ:
    /* Nothing to do. */
    break;

  case IO_METHOD_MMAP:
    for (i = 0; i < n_buffers; ++i) {
      struct v4l2_buffer buf;

      CLEAR (buf);

      buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory      = V4L2_MEMORY_MMAP;
      buf.index       = i;

      if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
        errno_exit ("VIDIOC_QBUF");
    }
    
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
      errno_exit ("VIDIOC_STREAMON");

    break;

  case IO_METHOD_USERPTR:
    for (i = 0; i < n_buffers; ++i) {
      struct v4l2_buffer buf;

      CLEAR (buf);

      buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory      = V4L2_MEMORY_USERPTR;
      buf.index       = i;
      buf.m.userptr = (unsigned long) buffers[i].start;
      buf.length      = buffers[i].length;

      if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
        errno_exit ("VIDIOC_QBUF");
    }

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
      errno_exit ("VIDIOC_STREAMON");

    break;
  }
}

static void
uninit_device (void)
{
  unsigned int i;

  switch (io) {
  case IO_METHOD_READ:
    free (buffers[0].start);
    break;

  case IO_METHOD_MMAP:
    for (i = 0; i < n_buffers; ++i)
      if (-1 == munmap (buffers[i].start, buffers[i].length))
        errno_exit ("munmap");
    break;

  case IO_METHOD_USERPTR:
    for (i = 0; i < n_buffers; ++i)
      free (buffers[i].start);
    break;
  }

  free (buffers);
}

static void
init_read (unsigned int   buffer_size)
{
  buffers = calloc (1, sizeof (*buffers));

  if (!buffers) {
    fprintf (stderr, "Out of memory\n");
    exit (EXIT_FAILURE);
  }

  buffers[0].length = buffer_size;
  buffers[0].start = malloc (buffer_size);

  if (!buffers[0].start) {
    fprintf (stderr, "Out of memory\n");
    exit (EXIT_FAILURE);
  }
}

static void
init_mmap (void)
{
  struct v4l2_requestbuffers req;

  CLEAR (req);

  req.count               = 4;
  req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory              = V4L2_MEMORY_MMAP;

  if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
    if (EINVAL == errno) {
      fprintf (stderr, "%s does not support "
               "memory mapping\n", dev_name);
      exit (EXIT_FAILURE);
    } else {
      errno_exit ("VIDIOC_REQBUFS");
    }
  }

  if (req.count < 2) {
    fprintf (stderr, "Insufficient buffer memory on %s\n",
             dev_name);
    exit (EXIT_FAILURE);
  }

  buffers = calloc (req.count, sizeof (*buffers));

  if (!buffers) {
    fprintf (stderr, "Out of memory\n");
    exit (EXIT_FAILURE);
  }

  for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
    struct v4l2_buffer buf;

    CLEAR (buf);

    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory      = V4L2_MEMORY_MMAP;
    buf.index       = n_buffers;

    if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
      errno_exit ("VIDIOC_QUERYBUF");

    buffers[n_buffers].length = buf.length;
    buffers[n_buffers].start =
      mmap (NULL /* start anywhere */,
            buf.length,
            PROT_READ | PROT_WRITE /* required */,
            MAP_SHARED /* recommended */,
            fd, buf.m.offset);

    if (MAP_FAILED == buffers[n_buffers].start)
      errno_exit ("mmap");
  }
}

static void
init_userp (unsigned int   buffer_size)
{
  struct v4l2_requestbuffers req;
  unsigned int page_size;

  page_size = getpagesize ();
  buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);

  CLEAR (req);

  req.count               = 4;
  req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory              = V4L2_MEMORY_USERPTR;

  if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
    if (EINVAL == errno) {
      fprintf (stderr, "%s does not support "
               "user pointer i/o\n", dev_name);
      exit (EXIT_FAILURE);
    } else {
      errno_exit ("VIDIOC_REQBUFS");
    }
  }

  buffers = calloc (4, sizeof (*buffers));

  if (!buffers) {
    fprintf (stderr, "Out of memory\n");
    exit (EXIT_FAILURE);
  }

  for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
    buffers[n_buffers].length = buffer_size;
    buffers[n_buffers].start = memalign (/* boundary */ page_size,
                                         buffer_size);

    if (!buffers[n_buffers].start) {
      fprintf (stderr, "Out of memory\n");
      exit (EXIT_FAILURE);
    }
  }
}

static void
init_device (void)
{
  struct v4l2_capability cap;
  struct v4l2_cropcap cropcap;
  struct v4l2_crop crop;
  struct v4l2_format fmt;
  struct v4l2_input input;
  int index;
  unsigned int min;

  if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) {
    if (EINVAL == errno) {
      fprintf (stderr, "%s is no V4L2 device\n",
               dev_name);
      exit (EXIT_FAILURE);
    } else {
      errno_exit ("VIDIOC_QUERYCAP");
    }
  }

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    fprintf (stderr, "%s is no video capture device\n",
             dev_name);
    exit (EXIT_FAILURE);
  }

  if (-1 == ioctl (fd, VIDIOC_G_INPUT, &index)) {
    perror ("VIDIOC_G_INPUT");
    exit (EXIT_FAILURE);
  }

  memset (&input, 0, sizeof (struct v4l2_input));
  input.index = index;

  if (-1 == ioctl (fd, VIDIOC_ENUMINPUT, &input)) {
    perror ("VIDIOC_ENUMINPUT");
    exit (EXIT_FAILURE);
  }

  fprintf (stderr, "Current input: %s\n", input.name);

  switch (io) {
  case IO_METHOD_READ:
    if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
      fprintf (stderr, "%s does not support read i/o\n",
               dev_name);
      exit (EXIT_FAILURE);
    }

    break;

  case IO_METHOD_MMAP:
  case IO_METHOD_USERPTR:
    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
      fprintf (stderr, "%s does not support streaming i/o\n",
               dev_name);
      exit (EXIT_FAILURE);
    }

    break;
  }


  /* Select video input, video standard and tune here. */

  /* If the input is not S-Video, set it */
  if (input.index != 2)
  {
    /* Set the video input */
    fprintf(stderr, "Attempting to set input from %s to S-Video\n", input.name);

    index = 2;

    if (-1 == xioctl (fd, VIDIOC_S_INPUT, &index))
    {
      perror ("VIDIOC_S_INPUT");
      exit (EXIT_FAILURE);
    }

    fprintf(stderr, "Set input to S-Video\n");
  }

  CLEAR (cropcap);

  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap))
  {
    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    crop.c = cropcap.defrect; /* reset to default */

    //crop.c.left += 4;
    //crop.c.top += 4;

    if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop))
    {
      switch (errno) {
      case EINVAL:
        /* Cropping not supported. */
        fprintf(stderr, "EINVAL when calling S_CROP\n");
        break;
      default:
        /* Errors ignored. */
        fprintf(stderr, "Unknown error\n");
        break;
      }
    }
  }
  else
  {  
    /* Errors ignored. */
  }


  CLEAR (fmt);

  fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width       = WIDTH;
  fmt.fmt.pix.height      = HEIGHT;
  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB32;//V4L2_PIX_FMT_YUYV;
  fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;

  if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
    errno_exit ("VIDIOC_S_FMT");

  /* Note VIDIOC_S_FMT may change width and height. */

  /* Buggy driver paranoia. */
  min = fmt.fmt.pix.width * 2;
  if (fmt.fmt.pix.bytesperline < min)
    fmt.fmt.pix.bytesperline = min;
  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  if (fmt.fmt.pix.sizeimage < min)
    fmt.fmt.pix.sizeimage = min;

  fprintf(stderr, "Width/Heighth Format (%d/%d %d)\n",
          fmt.fmt.pix.width,
          fmt.fmt.pix.height,
          fmt.fmt.pix.pixelformat);
  switch (io) {
  case IO_METHOD_READ:
    init_read (fmt.fmt.pix.sizeimage);
    break;

  case IO_METHOD_MMAP:
    init_mmap ();
    break;

  case IO_METHOD_USERPTR:
    init_userp (fmt.fmt.pix.sizeimage);
    break;
  }
}

static void
close_device (void)
{
  if (-1 == close (fd))
    errno_exit ("close");

  fd = -1;
}

static void
open_device (void)
{
  struct stat st; 

  if (-1 == stat (dev_name, &st)) {
    fprintf (stderr, "Cannot identify '%s': %d, %s\n",
             dev_name, errno, strerror (errno));
    exit (EXIT_FAILURE);
  }

  if (!S_ISCHR (st.st_mode)) {
    fprintf (stderr, "%s is no device\n", dev_name);
    exit (EXIT_FAILURE);
  }

  fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);

  if (-1 == fd) {
    fprintf (stderr, "Cannot open '%s': %d, %s\n",
             dev_name, errno, strerror (errno));
    exit (EXIT_FAILURE);
  }
}

static void
usage (FILE *fp, int argc, char **argv)
{
  fprintf (fp,
           "Usage: %s [options]\n\n"
           "Options:\n"
           "-d | --device name   Video device name [/dev/video]\n"
           "-h | --help          Print this message\n"
           "-m | --mmap          Use memory mapped buffers\n"
           "-r | --read          Use read() calls\n"
           "-u | --userp         Use application allocated buffers\n"
           "",
           argv[0]);
}

static const char short_options [] = "d:hmru";

static const struct option
long_options [] = {
  { "device",     required_argument,      NULL,           'd' },
  { "help",       no_argument,            NULL,           'h' },
  { "mmap",       no_argument,            NULL,           'm' },
  { "read",       no_argument,            NULL,           'r' },
  { "userp",      no_argument,            NULL,           'u' },
  { 0, 0, 0, 0 }
};

int
main (int argc, char **argv)
{
  dev_name = "/dev/video";

  for (;;)
  {
    int index;
    int c;
                
    c = getopt_long (argc, argv,
                     short_options, long_options,
                     &index);

    if (-1 == c)
      break;

    switch (c) {
    case 0: /* getopt_long() flag */
      break;

    case 'd':
      dev_name = optarg;
      break;

    case 'h':
      usage (stdout, argc, argv);
      exit (EXIT_SUCCESS);

    case 'm':
      io = IO_METHOD_MMAP;
      break;

    case 'r':
      io = IO_METHOD_READ;
      break;

    case 'u':
      io = IO_METHOD_USERPTR;
      break;

    default:
      usage (stderr, argc, argv);
      exit (EXIT_FAILURE);
    }
  }

  open_device ();

  init_device ();
  
  init_sdl ();

  rci_init ();

  start_capturing ();

  mainloop ();

  stop_capturing ();

  uninit_device ();

  rci_uninit ();

  SDL_Quit();

  close_device ();

  return 0;
}
